#!  /usr/bin/env python
# coding=utf-8

# import socket
# import os
# import json

# from serial_demo.msg import ti5_polyJC_Lists
# from serial_demo.msg import ti5_motorStatus

import numpy as np
import math
"""
注：机械臂实际基座高175，三维模型高190，逆解按照模型计算
即若想实物机械臂达到(x,y,z)则给定的指令应是(x,y,z+15)
"""
L0 = 190
L2 = 400
L3 = 300
LT = 300

# MDH
# alfa_i_1 = np.zeros((6,1),np.float)
# a_i_1 = np.zeros((6,1),np.float)
# theta_i = np.zeros((6,1),np.float)
# d_i = np.zeros((6,1),np.float)

pi = math.pi

# alfa_i_1[0]= 0 ;     a_i_1[0] = 0 ;    theta_i[0] = 0;       d_i[0] = 190

# alfa_i_1[1]= pi/2;   a_i_1[1] = 0;     theta_i[1] = pi/2;    d_i[1] = 0

# alfa_i_1[2]= 0;      a_i_1[2] = 400;   theta_i[2] = -pi/2;   d_i[2] = 0

# alfa_i_1[3]= -pi/2;  a_i_1[3] = 0;     theta_i[3] = 0;       d_i[3] = 300

# alfa_i_1[4]= pi/2;   a_i_1[4] = 0;     theta_i[4] = 0;       d_i[4] = 0

# alfa_i_1[5]= -pi/2;   a_i_1[5] = 0;     theta_i[5] = 0;       d_i[5] = 300


Tw_00 = np.eye(4, dtype = np.float)
Tw_00[2,3] = L0

T6_t = np.eye(4, dtype = np.float)
T6_t[2,3] = LT

R00_6 = np.eye(3,dtype= np.float)


class MDH_TF_f:
    @staticmethod
    def T_fram(alfa, a, theta, d):
        # type:(np.ndarray, np.ndarray, np.ndarray, np.ndarray)->np.ndarray
        T = np.zeros((6,4,4),dtype= np.float)
        for i in range(0,4):
            T[i,:,:] = np.eye(4, dtype=np.float)
            pass
        
        for i in range(0,6):
            T[i,0,0] = math.cos(theta[i])
            T[i,0,1] = -math.sin(theta[i])
            T[i,0,2] = 0.0
            T[i,0,3] = a[i]
            T[i,1,0] = math.sin(theta[i]) * math.cos(alfa[i])
            T[i,1,1] = math.cos(theta[i]) * math.cos(alfa[i])
            T[i,1,2] = -math.sin(alfa[i])
            T[i,1,3] = -math.sin(alfa[i]) * d[i]
            T[i,2,0] = math.sin(theta[i]) * math.sin(alfa[i])
            T[i,2,1] = math.cos(theta[i]) * math.sin(alfa[i])
            T[i,2,2] = math.cos(alfa[i])
            T[i,2,3] = math.cos(alfa[i]) * d[i]
            pass
        return T
        pass

    # 0 <= a < b <= n  Tf = array(n,4,4) has transfor of fram i-1 to i
    @staticmethod
    def T_fram_a_b(a,b,Tf):
        # type:(int, int, np.ndarray)->np.ndarray
        
        T = np.eye(4, dtype=np.float)
        for i in range(0,b-a):
            T = np.dot(T , Tf[a+i,:,:])
            pass

        return T
        pass
    
    @staticmethod
    def R_fram_0_4(t1,t2,t3):
        # type:(float,float,float)->np.ndarray
    
        R = np.eye(3,dtype= np.float)
        R[0,0] = math.cos(t1)*math.cos(t2)*math.cos(t3) - math.cos(t1)*math.sin(t2)*math.sin(t3)
        R[0,1] = -math.sin(t1)
        R[0,2] = -math.cos(t1)*math.cos(t2)*math.sin(t3) - math.cos(t1)*math.sin(t2)*math.cos(t3)
        R[1,0] = math.sin(t1)*math.cos(t2)*math.cos(t3) - math.sin(t1)*math.sin(t2)*math.sin(t3)
        R[1,1] = math.cos(t1)
        R[1,2] = -math.sin(t1)*math.cos(t2)*math.sin(t3) - math.sin(t1)*math.sin(t2)*math.cos(t3)
        R[2,0] = math.sin(t2)*math.cos(t3) + math.cos(t2)*math.sin(t3)
        R[2,1] = 0
        R[2,2] = -math.sin(t2)*math.sin(t3) + math.cos(t2)*math.cos(t3)


        return R
        pass

    @staticmethod
    def R_fram_0_4_inv(t1,t2,t3):
        # type:(float,float,float)->np.ndarray

        Rstar = np.eye(3,dtype= np.float) # R*
        Rstar[0,0] = math.cos(t1)*math.cos(t2+t3)
        Rstar[0,1] = math.sin(t1)*math.cos(t2+t3)
        Rstar[0,2] = math.sin(t2+t3)
        Rstar[1,0] = -math.sin(t1)
        Rstar[1,1] = math.cos(t1)
        Rstar[1,2] = 0
        Rstar[2,0] = -math.cos(t1)*math.sin(t2+t3) 
        Rstar[2,1] = -math.sin(t1)*math.sin(t2+t3)
        Rstar[2,2] = math.cos(t2+t3)

        return Rstar
        pass

    pass


# 24/3/6 修改了D-H参数
def ti5_ik(cJdeg, Targ):
    # type:(np.ndarray,np.ndarray)->np.ndarray
    """ti5逆解脚本
        input:current joint
              target pose/(rotM,Pverct/mm)
        output: solution/rad
    """
    theta = np.zeros((8,6),np.float)
  
    px = Targ[0,3]-Targ[0,2]*LT
    py = Targ[1,3]-Targ[1,2]*LT
    pz = Targ[2,3]-Targ[2,2]*LT - L0

    R00_6[:] = Targ[0:3,0:3]
    
 

    # theta1 1 solution
    if abs(px) <= 0.0001 and abs(py) <= 0.0001: # x,y=0,0
        theta1_1 = cJdeg[0]
    else:
        theta1_1 = math.atan2(py,px)
        pass

    # theta1 2 solution
    if theta1_1>0:
        theta1_2 = theta1_1 - pi
    else:
        theta1_2 = theta1_1 + pi  
        pass
    
    # theta2,3 
    xx = math.sqrt(px*px + py*py)
    
    LL = px*px + py*py + pz*pz
    L = math.sqrt(LL)

    fa = math.atan2(pz,xx)

    if abs(L2+L3-L)<=0.001:
        # only one solution
        theta2_1 = -pi/2+fa
        theta3_1 = 0
        theta2_2 = -pi/2+fa
        theta3_2 = 0     

    else:
        alfa = math.acos((LL + L2*L2 - L3*L3)/(2*L*L2))
        beta = math.acos((L2*L2 + L3*L3 - LL)/(2*L2*L3))
        
        # 1 solution
        theta2_1 = alfa+fa-pi/2
        theta3_1 = beta-pi
        # 2 solution
        theta2_2 = -alfa+fa-pi/2
        theta3_2 = -beta+pi
      
        pass

    # front 3 axes
    # 1
    theta[0,0] = theta1_1
    theta[0,1] = theta2_1
    theta[0,2] = theta3_1

    theta[4,0] = theta1_1
    theta[4,1] = theta2_1
    theta[4,2] = theta3_1

    # 2
    theta[1,0] = theta1_2
    theta[1,1] = -theta2_1
    theta[1,2] = -theta3_1

    theta[5,0] = theta1_2
    theta[5,1] = -theta2_1
    theta[5,2] = -theta3_1

    # 3
    theta[2,0] = theta1_1
    theta[2,1] = theta2_2
    theta[2,2] = theta3_2

    theta[6,0] = theta1_1
    theta[6,1] = theta2_2
    theta[6,2] = theta3_2

    # 4
    theta[3,0] = theta1_2
    theta[3,1] = -theta2_2
    theta[3,2] = -theta3_2

    theta[7,0] = theta1_2
    theta[7,1] = -theta2_2
    theta[7,2] = -theta3_2

    # theta4,5,6******************

    # solve last 3 axes by front 3 axes
    for j in range(0,4):
        th = np.zeros(3,np.float)
      
        th = theta[j,0:3] 
           
        R00_4_inv = MDH_TF_f.R_fram_0_4_inv(th[0],th[1],th[2])
        
        R4_6 = np.dot(R00_4_inv, R00_6)  # R4_6=(R00_4^-1)*R00_6
        R4_6_33 = R4_6[2,2]

        if (abs(R4_6_33-1)>0.0001) and ( abs(R4_6_33+1)>0.0001): # !=1 and !=-1
            beta = math.atan2(math.sqrt(R4_6[2,0]*R4_6[2,0] + R4_6[2,1]*R4_6[2,1]), R4_6_33)
            alfa = math.atan2(-R4_6[1,2], -R4_6[0,2])
            gama = math.atan2(-R4_6[2,1], R4_6[2,0])
        elif abs(R4_6_33-1)<=0.0001: # =1
            alfa = 0
            beta = 0
            gama = math.atan2(-R4_6[0,1],R4_6[1,1])
        elif abs(R4_6_33+1)<=0.0001: # =-1
            alfa = 0
            beta = pi
            gama = math.atan2(R4_6[0,1],R4_6[1,1])
            pass

        # 1 solution
        theta4_1 = alfa
        theta5_1 = beta #
        theta6_1 = gama
     
        # 2 solution
        theta5_2 = -theta5_1

        if theta4_1>=0:
            theta4_2 = theta4_1-pi
        else:
            theta4_2 = theta4_1+pi   
            pass

        if theta6_1>=0:
            theta6_2 = theta6_1-pi
        else:
            theta6_2 = theta6_1+pi   
            pass
        
        # each set of solution for the front 3 axes 
        # corresponds to 2 sets of solution for the last 3 axes
        theta[j,3] = theta4_1
        theta[j,4] = theta5_1
        theta[j,5] = theta6_1
        
        theta[j+4,3] = theta4_2
        theta[j+4,4] = theta5_2
        theta[j+4,5] = theta6_2

        pass # end of for j in range(0,4):
    
  
    # print(np.around(theta*180/pi,3))
    return theta
    pass


def ti5_optIK(cJdeg, Targ):
    # type:(np.ndarray,np.ndarray)->np.ndarray
    """input:cJdeg/deg, Targ/mm
    ouput:jointIK/deg"""
    try:
        theta = ti5_ik(cJdeg, Targ)
    except Exception as e:
        print('error occurred in ti5_optIK24()')
        print(e)
        return cJdeg
        

    numOfSolution = np.size(theta)/6
    theta = theta*(180/pi)
    
    wct = [4,12,6,2,2,1]
    if(0 < numOfSolution): 
        while(numOfSolution > 1):
            minusTheta = abs(theta - cJdeg)
           
            timesTheta = minusTheta * wct
          
            sumTheta = np.sum(timesTheta,axis=1)
           
            minJindex = np.argmin(sumTheta)
            
            if abs(theta[minJindex,0])-170<=0 and abs(theta[minJindex,3])-170<=0 and abs(theta[minJindex,1])-90<=0 \
            and abs(theta[minJindex,2])-150<=0 and abs(theta[minJindex,4])-150<=0 and abs(theta[minJindex,5])-180<=0 :
                bestR = theta[minJindex,:]
                return bestR
                break
            else:
                theta = np.delete(theta,obj=minJindex, axis=0) 
                numOfSolution = numOfSolution-1
            pass # while(numOfSolution > 1)
       
        if(1 == numOfSolution):
           
            theta=np.ravel(theta)
            if abs(theta[0])-170<=0 and abs(theta[3])-170<=0 and abs(theta[1])-90<=0 \
            and abs(theta[2])-150<=0 and theta[4]-200<=0 and theta[4]+50>=0 \
            and abs(theta[5])-180<=0 :
                bestR = theta
            else:
                return cJdeg  
    else:
        return cJdeg
        pass
    
    return cJdeg

    pass


def TFMeef(xyz_eulZYX):
    # type:(np.ndarray)->np.ndarray
    """input xyz unit is mm angle unit is rad"""
    alfa = xyz_eulZYX[3]
    beta = xyz_eulZYX[4]
    gama = xyz_eulZYX[5]

    # 注意：此处T是浅拷贝，会直接改变xyz_eulZYX的值
    T = np.eye(4,dtype= np.float)
    T[0:3,3] = xyz_eulZYX[0:3]

    T[0,0] = math.cos(alfa) * math.cos(beta) 
    T[0,1] = math.cos(alfa) * math.sin(beta) * math.sin(gama) - math.sin(alfa) * math.cos(gama) 
    T[0,2] = math.cos(alfa) * math.sin(beta) * math.cos(gama) + math.sin(alfa) * math.sin(gama)
    T[1,0] = math.sin(alfa) * math.cos(beta)
    T[1,1] = math.sin(alfa) * math.sin(beta) * math.sin(gama) + math.cos(alfa) * math.cos(gama) 
    T[1,2] = math.sin(alfa) * math.sin(beta) * math.cos(gama) - math.cos(alfa) * math.sin(gama)
    T[2,0] = -math.sin(beta)
    T[2,1] = math.cos(beta) * math.sin(gama)
    T[2,2] = math.cos(beta) * math.cos(gama)
    
    return T
    pass

def mdhTfM(alfa_i_1, a_i_1, theta_i, d_i):
    # type:(float, float, float, float)->np.ndarray
    """input angle unit is rad"""
    T = np.eye(4, dtype=np.float)
    T[0,0] = math.cos(theta_i)
    T[0,1] = -math.sin(theta_i)
    T[0,2] = 0
    T[0,3] = a_i_1
    T[1,0] = math.sin(theta_i)*math.cos(alfa_i_1)
    T[1,1] = math.cos(theta_i)*math.cos(alfa_i_1)
    T[1,2] = -math.sin(alfa_i_1)
    T[1,3] = -math.sin(alfa_i_1)*d_i
    T[2,0] = math.sin(theta_i)*math.sin(alfa_i_1)
    T[2,1] = math.cos(theta_i)*math.sin(alfa_i_1)
    T[2,2] = math.cos(alfa_i_1)
    T[2,3] = math.cos(alfa_i_1)*d_i
 
    return T
    pass


# 24/3/6  修改了D-H参数
def ti5FK(theta):
    # type:(np.ndarray)->np.ndarray
    """input angle unit is rad"""
    T01 = mdhTfM(0,       0,    0+theta[0],         190)
    T12 = mdhTfM(pi/2,    0,    pi/2+theta[1],      0)
    T23 = mdhTfM(0,       400,  -pi/2+theta[2],     0 )
    T34 = mdhTfM(-pi/2,   0,    0+theta[3],         300)
    T45 = mdhTfM(pi/2,    0,    0+theta[4],         0 )
    T56 = mdhTfM(-pi/2,   0,    0+theta[5],         300)
    Tf = np.dot(np.dot(np.dot(np.dot(np.dot(T01, T12), T23), T34), T45), T56)

    return Tf
    pass

def rotM2Euler_ZYX(r):
    # type:(np.ndarray)->np.ndarray
    """" input:rotation matrix
         output:Euler_ZYX/deg  """
    if(abs(r[2,0]-1)<=0.000001): # 1  beta = -pi/2
        alfa = 0
        beta = -pi/2
        gama = math.atan2(-r[0,1], r[1,1])# gama+alfa  if alfa=0 then this is gama
    elif(abs(r[2,0]+1)<=0.000001):# -1  beta = pi/2
        alfa = 0
        beta = pi/2
        gama = math.atan2(r[0,1], r[1,1])# gama-alfa  if alfa=0 then this is gama
      
    else:
        beta = math.atan2(-r[2,0], math.sqrt(r[2,1]**2+r[2,2]**2))
        alfa = math.atan2(r[1,0], r[0,0])
        gama = math.atan2(r[2,1], r[2,2])
        pass
    

    return np.array([alfa*180/pi, beta*180/pi ,gama*180/pi])
    pass


def ti5SimpleIK_p2j(cJdeg,tPose):
    """input:cJdeg/deg, tPose/mm_deg
        ouput:jointIK/deg
    """
    tPose[3] = tPose[3]*pi/180
    tPose[4] = tPose[4]*pi/180
    tPose[5] = tPose[5]*pi/180
    return ti5_optIK(cJdeg,Targ=TFMeef(tPose))

    pass

def ti5SimpleIK_t2j(cJdeg,Targ):
    
    return ti5_optIK(cJdeg,Targ)
    pass


def ti5_j2p(cJdeg):
    # type:(np.ndarray)->np.ndarray
    """ cJdeg to xyz_eulZYX/mm_deg
    input:cJdeg/deg
    otput:xyz_eulZYX/mm_deg """
    T = ti5FK(cJdeg*pi/180)
    x = T[0,3]
    y = T[1,3]
    z = T[2,3]
    eulZYX = np.array(rotM2Euler_ZYX(T[0:3,0:3]))

    return np.array([x,y,z,eulZYX[0],eulZYX[1],eulZYX[2]])
    pass


def paper_3_3_2_fktest():
    
    theta=np.array([[10,20,30,40,50,60],
                    [90,0,90,0,90,0],
                    [0,30,-30,0,0,30],
                    [60,-30,0,-20,-30,100],
                    [-30,-60,-20,24,33,-50],
                    [-22,11,45,-30,90,-25]])*pi/180
    
    for i in range(0,6):
        T = ti5FK(theta[i,:])
        xyz = T[0:3,3]
        eul_ZYX = rotM2Euler_ZYX(T[0:3,0:3])
        
        print("第{}组：{}".format(i,[xyz,eul_ZYX]))
        print(T)
    pass

def eulZYX2RotM(eulZYX):
    """欧拉角转旋转矩阵
        input:eulZYX/rad
        output:rotM
    """
    
    rotM = np.zeros((3,3))
    rotM[0,0] = math.cos(eulZYX[0])*math.cos(eulZYX[1])
    rotM[0,1] = math.cos(eulZYX[0])*math.sin(eulZYX[1])*math.sin(eulZYX[2]) - math.sin(eulZYX[0])*math.cos(eulZYX[2])
    rotM[0,2] = math.cos(eulZYX[0])*math.sin(eulZYX[1])*math.cos(eulZYX[2]) + math.sin(eulZYX[0])*math.sin(eulZYX[2])
    rotM[1,0] = math.sin(eulZYX[0])*math.cos(eulZYX[1])
    rotM[1,1] = math.sin(eulZYX[0])*math.sin(eulZYX[1])*math.sin(eulZYX[2]) + math.cos(eulZYX[0])*math.cos(eulZYX[2])
    rotM[1,2] = math.sin(eulZYX[0])*math.sin(eulZYX[1])*math.cos(eulZYX[2]) - math.cos(eulZYX[0])*math.sin(eulZYX[2])
    rotM[2,0] = -math.sin(eulZYX[1])
    rotM[2,1] = math.cos(eulZYX[1])*math.sin(eulZYX[2])
    rotM[2,2] = math.cos(eulZYX[1])*math.cos(eulZYX[2])
    
    return rotM
    pass

def xyz_eulZYX2TransM(xyz_eulZYX):
    """位置和欧拉角转齐次变换矩阵
        input:xyz_eulZYX/(mm,deg)
        output:TransM/(R3*3,xyz/m)
    """
    
    T = np.eye(4)
    
    T[0:3,0:3] = eulZYX2RotM(xyz_eulZYX[3:6]*pi/180)
    T[0:3,3] = np.squeeze(xyz_eulZYX[0:3])
    
    return T
    
    pass





if  __name__== "__main__":
   


    theta = np.array([0,0,-90,0,-90,-90])*pi/180

    
    fT = ti5FK(theta)
    
    euler_ZYX = rotM2Euler_ZYX(fT[0:3,0:3])
 
  
    
    iT = xyz_eulZYX2TransM(np.array([300,0,290,180,0,-180]))
    
    ik = ti5_ik(np.array([0,0,0,0,0,0])*pi/180, fT)
    
    optik = ti5_optIK(np.array([0,0,0,0,0,0])*pi/180, iT)
    
    # np.set_printoptions(formatter={'float': '{:.3f}'.format})
    np.set_printoptions(precision=3, suppress=True)
    print("\nfk:\n")
    print('\nfT:\n',fT)
    print('euler_ZYX:\n',euler_ZYX)
    print("\nik:\n")
    print("iT:\n",iT)
    print('ik:\n', ik*180/pi)
    print('optik:\n', optik)


    pass




    # Targ = np.array([[0,1,0,0],
    #                  [1,0,0,-300],
    #                  [0,0,-1,290],
    #                  [0,0,0,1]],dtype= np.float)
   

  


    # jd = ti5_optIK(cJdeg=np.array([-90,0,-90,0,0,-90],np.float),Targ=Targ)
    # print(np.around(jd,3))
    # T = TFMeef(np.array([450,200,50,90*pi/180,0,180*pi/180],np.float))
    # print(T)
    # jd = ti5_optIK(cJdeg=np.zeros((1,6),np.float),Targ=T)
    # print('opjd',np.around(jd,3))

    # theta = np.array([40.0000, -65.0000, -50.0000, -88.2700, 115.0000, 90.0000])*pi/180
    # T = ti5FK(theta)
    # print(T)
    # jd = ti5_optIK_print(cJdeg=np.zeros((1,6),np.float),Targ=T)
    # print('opjd',np.around(jd,3))



    # print(rospy.Time.now().to_nsec())
    # time2 = rospy.Time.now().to_nsec()
    # print((time2-time1)/1000000)

    # sPose = ti5_j2p(np.array([0,0,0,0,0,0]))
    # ePose = ti5_j2p(np.array([0,0,-90,0,0,0]))
    # print('sPose', sPose )
    # print('ePose', ePose )

    # from interPolyPointPub import move_p
    # from serial_demo.msg import ti5_polyJC_Lists
    # pub = rospy.Publisher("interPolyPoint_topic",ti5_polyJC_Lists,queue_size=1000)
    # move_p(sPose, ePose, pub=pub, cmd_rate=500, use_time=6,cJdeg=np.array([0,0,0,0,0,0]))




